/* cable_ft2232.c - FT2232 based cable driver for the Advanced JTAG Bridge
   Copyright (C) 2008 Arnim Laeuger, arniml@opencores.org
   Copyright (C) 2009 José Ignacio Villar, jose@dte.us.es
   
   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2 of the License, or
   (at your option) any later version.
   
   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
   
   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. */


#include <time.h>
#include <sys/time.h>
#include <fcntl.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <stdint.h>
#include <unistd.h>
#include <ftdi.h>

#include "cable_ft2232.h"
#include "errcodes.h"

#define debug(...) if(0) fprintf(stderr, __VA_ARGS__)

int debug = 0;

jtag_cable_t ft2232_cable_driver = 
{
    .name = "ft2232",
    .inout_func = NULL,
    .out_func = NULL,
    .init_func = cable_ftdi_init,
    .opt_func = cable_ftdi_opt,
    .bit_out_func = cable_ftdi_write_bit,
    .bit_inout_func = cable_ftdi_read_write_bit,
    .stream_out_func = cable_ftdi_write_stream,
    .stream_inout_func = cable_ftdi_read_stream,
    .flush_func = cable_ftdi_flush,
    .opts = "p:v:",
    .help = "-p [PID] Alteranate PID for USB device (hex value)\n\t-v [VID] Alternate VID for USB device (hex value)\n",
};

usbconn_t * usbconn_ftdi_connect();
int my_ftdi_write_data(struct ftdi_context *ftdi, unsigned char *buf, int size);
char *my_ftdi_get_error_string (struct ftdi_context *ftdi);
int my_ftdi_read_data(struct ftdi_context *ftdi, unsigned char *buf, int size);
int my_ftdi_usb_open_desc(struct ftdi_context *ftdi, int vendor, int product, const char* description, const char* serial);
void my_ftdi_deinit(struct ftdi_context *ftdi);
int my_ftdi_usb_purge_buffers(struct ftdi_context *ftdi);
int my_ftdi_usb_purge_rx_buffer(struct ftdi_context *ftdi);
int my_ftdi_usb_purge_tx_buffer(struct ftdi_context *ftdi);
int my_ftdi_usb_reset(struct ftdi_context *ftdi);
int my_ftdi_set_latency_timer(struct ftdi_context *ftdi, unsigned char latency);
int my_ftdi_set_baudrate(struct ftdi_context *ftdi, int baudrate);
int my_ftdi_read_data_set_chunksize(struct ftdi_context *ftdi, unsigned int chunksize);
int my_ftdi_write_data_set_chunksize(struct ftdi_context *ftdi, unsigned int chunksize);
int my_ftdi_set_event_char(struct ftdi_context *ftdi, unsigned char eventch, unsigned char enable);
int my_ftdi_set_error_char(struct ftdi_context *ftdi, unsigned char errorch, unsigned char enable);
int my_ftdi_set_bitmode(struct ftdi_context *ftdi, unsigned char bitmask, unsigned char mode);
int my_ftdi_usb_close(struct ftdi_context *ftdi);

static int usbconn_ftdi_common_open(usbconn_t *conn);
static void usbconn_ftdi_free(usbconn_t *conn);
static int seq_purge(struct ftdi_context *ftdic, int purge_rx, int purge_tx);
static int seq_reset(struct ftdi_context *ftdic);
static int usbconn_ftdi_flush(ftdi_param_t *params);
static int usbconn_ftdi_read(usbconn_t *conn, uint8_t *buf, int len);
static int usbconn_ftdi_write(usbconn_t *conn, uint8_t *buf, int len, int recv);
static int usbconn_ftdi_mpsse_open(usbconn_t *conn);
static int usbconn_ftdi_close(usbconn_t *conn);

usbconn_driver_t usbconn_ft2232_mpsse_driver = 
{
  "ftdi-mpsse",
  usbconn_ftdi_connect,
  usbconn_ftdi_free,
  usbconn_ftdi_mpsse_open,
  usbconn_ftdi_close,
  usbconn_ftdi_read,
  usbconn_ftdi_write
};

usbconn_cable_t usbconn_ft2232_mpsse_CableID2= 
{
  "Olimex ARM-USB-OCD-H",   /* cable name */
  "Olimex ARM-USB-OCD-H",   /* string pattern, not used */
  "ftdi-mpsse",             /* default usbconn driver */
  0x0403,                   /* VID */
  0x6010                    /* PID */
};

static usbconn_t *ft2232_device;


/// ----------------------------------------------------------------------------------------------
/// libftdi wrappers for debugging purposes.
/// ----------------------------------------------------------------------------------------------

void print_buffer(unsigned char *buf, int size) 
{
  int i = 0;
  for(i = 0; i < size; i++)
    printf("[MYDBG]\tBUFFER[%d] = %02x\n", i, buf[i]);

}

int my_ftdi_write_data(struct ftdi_context *ftdi, unsigned char *buf, int size) 
{
  debug("[MYDBG] ftdi_write_data(ftdi, buf=BUFFER[%d], size=%d);\n", size, size);
  if (debug > 1) 
    print_buffer(buf, size);
  return ftdi_write_data(ftdi, buf, size);
}

char *my_ftdi_get_error_string (struct ftdi_context *ftdi) 
{
  debug("[MYDBG] ftdi_get_error_string(ftdi);\n");
  return ftdi_get_error_string (ftdi);
}

int my_ftdi_read_data(struct ftdi_context *ftdi, unsigned char *buf, int size) 
{
  int ret = 0;
  debug("[MYDBG] ftdi_read_data(ftdi, buf=BUFFER[%d], size=%d);\n", size, size);
  ret = ftdi_read_data(ftdi, buf, size);
  if(debug) 
    print_buffer(buf, size);
  return ret;
}

int my_ftdi_usb_open_desc(struct ftdi_context *ftdi, int vendor, int product, const char* description, const char* serial) 
{
  debug("[MYDBG] ftdi_usb_open_desc(ftdi, vendor=%d, product=%d, description=DESCRIPTION, serial=SERIAL);\n", vendor, product);
  return ftdi_usb_open_desc(ftdi, vendor, product, description, serial);
}

void my_ftdi_deinit(struct ftdi_context *ftdi) 
{
  debug("[MYDBG] ftdi_deinit(ftdi);\n");
  ftdi_deinit(ftdi);
}

int my_ftdi_usb_purge_buffers(struct ftdi_context *ftdi) 
{
  debug("[MYDBG] ftdi_usb_purge_buffers(ftdi);\n");
  return ftdi_usb_purge_buffers(ftdi);
}

int my_ftdi_usb_purge_rx_buffer(struct ftdi_context *ftdi) 
{
  debug("[MYDBG] ftdi_usb_purge_rx_buffer(ftdi);\n");
  return ftdi_usb_purge_rx_buffer(ftdi);
}

int my_ftdi_usb_purge_tx_buffer(struct ftdi_context *ftdi) 
{
  debug("[MYDBG] ftdi_usb_purge_tx_buffer(ftdi);\n");
  return ftdi_usb_purge_tx_buffer(ftdi);
}

int my_ftdi_usb_reset(struct ftdi_context *ftdi) 
{
  debug("[MYDBG] ftdi_usb_reset(ftdi);\n");
  return ftdi_usb_reset(ftdi);
}

int my_ftdi_set_latency_timer(struct ftdi_context *ftdi, unsigned char latency) 
{
  debug("[MYDBG] ftdi_set_latency_timer(ftdi, latency=0x%02x);\n", latency);
  return ftdi_set_latency_timer(ftdi, latency);
}

int my_ftdi_set_baudrate(struct ftdi_context *ftdi, int baudrate) 
{
  debug("[MYDBG] ftdi_set_baudrate(ftdi, baudrate=%d);\n", baudrate);
  return ftdi_set_baudrate(ftdi, baudrate);
}

int my_ftdi_read_data_set_chunksize(struct ftdi_context *ftdi, unsigned int chunksize) 
{
  debug("[MYDBG] ftdi_read_data_set_chunksize(ftdi, chunksize=%u);\n", chunksize);
  return ftdi_read_data_set_chunksize(ftdi, chunksize);
}

int my_ftdi_write_data_set_chunksize(struct ftdi_context *ftdi, unsigned int chunksize) 
{
  debug("[MYDBG] ftdi_write_data_set_chunksize(ftdi, chunksize=%u);\n", chunksize);
  return ftdi_write_data_set_chunksize(ftdi, chunksize);
}

int my_ftdi_set_event_char(struct ftdi_context *ftdi, unsigned char eventch, unsigned char enable) 
{
  debug("[MYDBG] ftdi_set_event_char(ftdi, eventch=0x%02x, enable=0x%02x);\n", eventch, enable);
  return ftdi_set_event_char(ftdi, eventch, enable);
}

int my_ftdi_set_error_char(struct ftdi_context *ftdi, unsigned char errorch, unsigned char enable) 
{
  debug("[MYDBG] ftdi_set_error_char(ftdi, errorch=0x%02x, enable=0x%02x);\n", errorch, enable);
  return ftdi_set_error_char(ftdi, errorch, enable);
}

int my_ftdi_set_bitmode(struct ftdi_context *ftdi, unsigned char bitmask, unsigned char mode) 
{
  debug("[MYDBG] ftdi_set_bitmode(ftdi, bitmask=0x%02x, mode=0x%02x);\n", bitmask, mode);
  return ftdi_set_bitmode(ftdi, bitmask, mode);
}

int my_ftdi_usb_close(struct ftdi_context *ftdi) 
{
  debug("[MYDBG] ftdi_usb_close(ftdi);\n");
  return ftdi_usb_close(ftdi);
}



/// ----------------------------------------------------------------------------------------------
/// USBconn FTDI MPSSE subsystem
/// ----------------------------------------------------------------------------------------------

static int usbconn_ftdi_common_open(usbconn_t *conn) 
{
  ftdi_param_t *params = conn->params;
  struct ftdi_context * ftdic = params->ftdic;
  int error;

  printf("Initializing USB device\n");
  
  if ((error = my_ftdi_usb_open_desc(ftdic, conn->cable->vid, conn->cable->pid, NULL, NULL))) 
  {
    if      (error == -1) printf("usb_find_busses() failed\n");
    else if (error == -2) printf("usb_find_devices() failed\n");
    else if (error == -3) printf("usb device not found with VID 0x%0X, PID 0x%0X\n", conn->cable->vid, conn->cable->pid);
    else if (error == -4) printf("unable to open device\n");
    else if (error == -5) printf("unable to claim device\n");
    else if (error == -6) printf("reset failed\n");
    else if (error == -7) printf("set baudrate failed\n");
    else if (error == -8) printf("get product description failed\n");
    else if (error == -9) printf("get serial number failed\n");
    else if (error == -10) printf("unable to close device\n");

    my_ftdi_deinit(ftdic);
    ftdic = NULL;

    printf("Can't open FTDI usb device\n"); 
    return(-1);
  }

  return 0;
}

static int seq_purge(struct ftdi_context *ftdic, int purge_rx, int purge_tx) 
{
  int r = 0;
  unsigned char buf;

  if ((r = my_ftdi_usb_purge_buffers(ftdic)) < 0)
    printf("my_ftdi_usb_purge_buffers() failed\n");
  if (r >= 0) if ((r = my_ftdi_read_data(ftdic, &buf, 1)) < 0)
    printf("my_ftdi_read_data() failed\n");

  return r < 0 ? -1 : 0;
}

static int seq_reset(struct ftdi_context *ftdic) 
{
  if (my_ftdi_usb_reset(ftdic) < 0) 
  {
    printf("my_ftdi_usb_reset() failed\n");
    return -1;
  }

  if(seq_purge(ftdic, 1, 1) < 0)
    return -1;

  return 0;
}

static int usbconn_ftdi_flush(ftdi_param_t *params)
{
  int xferred;
  int recvd = 0;

  if (!params->ftdic)
    return -1;

  if (params->send_buffered == 0)
    return 0;

  if ((xferred = my_ftdi_write_data(params->ftdic, params->send_buf, params->send_buffered)) < 0)
    printf("my_ftdi_write_data() failed\n");

  if (xferred < params->send_buffered) 
  {
    printf("Written fewer bytes than requested.\n");
    return -1;
  }

  params->send_buffered = 0;

  /* now read all scheduled receive bytes */
  if (params->to_recv) 
  {
    if (params->recv_write_idx + params->to_recv > params->recv_buf_len) 
    {
      /* extend receive buffer */
      params->recv_buf_len = params->recv_write_idx + params->to_recv;
      if (params->recv_buf)
        params->recv_buf = (uint8_t *)realloc(params->recv_buf, params->recv_buf_len);
    }

    if (!params->recv_buf) 
    {
      printf("Receive buffer does not exist.\n");
      return -1;
    }

    while (recvd == 0)
      if ((recvd = my_ftdi_read_data(params->ftdic, &(params->recv_buf[params->recv_write_idx]), params->to_recv)) < 0)
        printf("Error from my_ftdi_read_data()\n");

    if (recvd < params->to_recv)
      printf("Received less bytes than requested.\n");

    params->to_recv -= recvd;
    params->recv_write_idx += recvd;
  }

  debug("[MYDBG] FLUSHING xferred=%u\n", xferred);
  return xferred < 0 ? -1 : xferred;
}

static int usbconn_ftdi_read(usbconn_t *conn, uint8_t *buf, int len) 
{
  ftdi_param_t *params = conn->params;
  int cpy_len;
  int recvd = 0;

  if (!params->ftdic)
    return -1;

  /* flush send buffer to get all scheduled receive bytes */
  if (usbconn_ftdi_flush(params) < 0)
    return -1;

  if (len == 0)
    return 0;

  /* check for number of remaining bytes in receive buffer */
  cpy_len = params->recv_write_idx - params->recv_read_idx;
  if (cpy_len > len)
    cpy_len = len;
  len -= cpy_len;

  if (cpy_len > 0) 
  {
    /* get data from the receive buffer */
    memcpy(buf, &(params->recv_buf[params->recv_read_idx]), cpy_len);
    params->recv_read_idx += cpy_len;
    if (params->recv_read_idx == params->recv_write_idx)
      params->recv_read_idx = params->recv_write_idx = 0;
  }

  if (len > 0) 
  {
    /* need to get more data directly from the device */
    while (recvd == 0)
      if ((recvd = my_ftdi_read_data(params->ftdic, &(buf[cpy_len]), len)) < 0)
        printf("Error from my_ftdi_read_data()\n");
  } 
  debug("[MYDBG] READ cpy_len=%u ; len=%u\n", cpy_len, len);
  return recvd < 0 ? -1 : cpy_len + len;
}

static int usbconn_ftdi_write(usbconn_t *conn, uint8_t *buf, int len, int recv) 
{
  ftdi_param_t *params = conn->params;
  int xferred = 0;

  if (!params->ftdic)
    return -1;

  /* this write function will try to buffer write data
     buffering will be ceased and a flush triggered in two cases. */

  /* Case A: max number of scheduled receive bytes will be exceeded
     with this write
     Case B: max number of scheduled send bytes has been reached */
  if ((params->to_recv + recv > FTDI_MAXRECV) || ((params->send_buffered > FTDX_MAXSEND) && (params->to_recv == 0)))
    xferred = usbconn_ftdi_flush(params);

  if (xferred < 0)
    return -1;

  /* now buffer this write */
  if (params->send_buffered + len > params->send_buf_len) 
  {
    params->send_buf_len = params->send_buffered + len;
    if (params->send_buf)
      params->send_buf = (uint8_t *)realloc(params->send_buf, params->send_buf_len);
  }

  if (params->send_buf) 
  {
    memcpy(&(params->send_buf[params->send_buffered]), buf, len);
    params->send_buffered += len;
    if (recv > 0)
      params->to_recv += recv;

    /* immediate write requested, so flush the buffered data */
    if (recv < 0) 
      xferred = usbconn_ftdi_flush(params);

    debug("[MYDBG] WRITE inmediate=%s ; xferred=%u ; len=%u\n", ((recv < 0) ? "TRUE" : "FALSE"), xferred, len);
    return xferred < 0 ? -1 : len;
  }
  else 
  {
    printf("Send buffer does not exist.\n");
    return -1;
  }
}

static int usbconn_ftdi_mpsse_open(usbconn_t *conn) 
{
  ftdi_param_t *params = conn->params;
  struct ftdi_context *ftdic = params->ftdic;
  
  int r = 0;

  if (usbconn_ftdi_common_open(conn) < 0) 
  {
    printf("Connection failed\n");
    return -1;
  }

  /* This sequence might seem weird and containing superfluous stuff.
     However, it's built after the description of JTAG_InitDevice
     Ref. FTCJTAGPG10.pdf
     Intermittent problems will occur when certain steps are skipped. */

  r = seq_reset(ftdic);
  if (r >= 0)
    r = seq_purge(ftdic, 1, 0);

  if (r >= 0)
    if ((r = my_ftdi_write_data_set_chunksize(ftdic, FTDX_MAXSEND_MPSSE)) < 0)
      puts(my_ftdi_get_error_string(ftdic));

  if (r >= 0)
    if ((r = my_ftdi_read_data_set_chunksize(ftdic, FTDX_MAXSEND_MPSSE)) < 0)
      puts(my_ftdi_get_error_string(ftdic));

  /* set a reasonable latency timer value
     if this value is too low then the chip will send intermediate result data
     in short packets (suboptimal performance) */
  if (r >= 0)
    if ((r = my_ftdi_set_latency_timer(ftdic, 16)) < 0)
      printf("my_ftdi_set_latency_timer() failed\n");

  if (r >= 0)
    if ((r = my_ftdi_set_bitmode(ftdic, 0x0b, BITMODE_MPSSE)) < 0)
      printf("my_ftdi_set_bitmode() failed\n");

  if (r >= 0)
    if ((r = my_ftdi_usb_reset(ftdic)) < 0)
      printf("my_ftdi_usb_reset() failed\n");

  if (r >= 0) 
    r = seq_purge(ftdic, 1, 0);

  /* set TCK Divisor */
  if (r >= 0) 
  {
    uint8_t buf[3] = {TCK_DIVISOR, 0x00, 0x00};
    r = usbconn_ftdi_write(conn, buf, 3, 0);
  }

  /* switch off loopback */
  if (r >= 0) 
  {
    uint8_t buf[1] = {LOOPBACK_END};
    r = usbconn_ftdi_write(conn, buf, 1, 0);
  }

  if (r >= 0)
    r = usbconn_ftdi_read(conn, NULL, 0);

  if (r >= 0)
    if ((r = my_ftdi_usb_reset(ftdic)) < 0)
      printf("my_ftdi_usb_reset() failed\n");

  if (r >= 0)
    r = seq_purge(ftdic, 1, 0);

  if (r < 0) 
  {
    ftdi_usb_close(ftdic);
    ftdi_deinit(ftdic);
    /* mark ftdi layer as not initialized */
    params->ftdic = NULL;
  }

  return r < 0 ? -1 : 0;
}

static int usbconn_ftdi_close(usbconn_t *conn) 
{
  ftdi_param_t *params = conn->params;

  if (params->ftdic) 
  {
    my_ftdi_usb_close(params->ftdic);
    my_ftdi_deinit(params->ftdic);
    params->ftdic = NULL;
  }

  return 0;
}

static void usbconn_ftdi_free(usbconn_t *conn)
{
  ftdi_param_t *params = conn->params;

  if (params->send_buf) free(params->send_buf);
  if (params->recv_buf) free(params->recv_buf);
  if (params->ftdic)    free(params->ftdic);
  if (params->serial)   free(params->serial);

  free(conn->params);
  free(conn);
}

usbconn_t * usbconn_ftdi_connect() 
{
  usbconn_t *conn            = malloc(sizeof(usbconn_t));
  ftdi_param_t *params       = malloc(sizeof(ftdi_param_t));
  struct ftdi_context *ftdic = malloc(sizeof(struct ftdi_context));

  if (params) 
  {
    params->send_buf_len   = FTDX_MAXSEND;
    params->send_buffered  = 0;
    params->send_buf       = (uint8_t *) malloc(params->send_buf_len);
    params->recv_buf_len   = FTDI_MAXRECV;
    params->to_recv        = 0;
    params->recv_write_idx = 0;
    params->recv_read_idx  = 0;
    params->recv_buf       = (uint8_t *) malloc(params->recv_buf_len);
  }

  if (!conn || !params || !ftdic || !params->send_buf || !params->recv_buf) 
  {
    printf("Can't allocate memory for ftdi context structures\n");

    if (conn) free(conn);
    if (params) free(params);
    if (ftdic) free(ftdic);
    if (params->send_buf) free(params->send_buf);
    if (params->recv_buf) free(params->recv_buf);
    return NULL;
  }

  conn->driver = &usbconn_ft2232_mpsse_driver;
  conn->cable  = &usbconn_ft2232_mpsse_CableID2;

  ftdi_init(ftdic);
  params->ftdic  = ftdic;
  params->pid    = conn->cable->pid;
  params->vid    = conn->cable->vid;
  params->serial = NULL;

  conn->params = params;

  printf("Structs successfully initialized\n");

  /* do a test open with the specified cable paramters,
     alternatively we could use libusb to detect the presence of the
     specified USB device   */
  if (usbconn_ftdi_common_open(conn) != 0) 
  {
    printf("Connection failed\n");
    usbconn_ftdi_free(conn);
    printf("Freeing structures.\n");
    return NULL;
  }

  my_ftdi_usb_close(ftdic);

  printf("Connected to libftdi driver.\n");

  return conn;
}



/// ----------------------------------------------------------------------------------------------
/// High level functions to generate Tx/Rx commands
/// ----------------------------------------------------------------------------------------------

int cable_ft2232_write_bytes(usbconn_t *conn, unsigned char *buf, int len, int postread) 
{
  int cur_command_size;
  int max_command_size;
  int cur_chunk_len;
  int recv;
  int xferred;
  int i;
  unsigned char *mybuf;

  if(len == 0)
    return 0;
  debug("write_bytes(length=%d, postread=%s)\n", len, ((postread > 0) ? "TRUE" : "FALSE"));
  recv = 0;
  max_command_size = min(len, 65536)+3;
  mybuf = (unsigned char *) malloc(max_command_size);

  /// Command OPCODE: write bytes
  mybuf[0] = MPSSE_DO_WRITE | MPSSE_LSB | MPSSE_WRITE_NEG;
  if(postread) // if postread is enabled it will buffer incoming bytes
    mybuf[0] = mybuf[0] | MPSSE_DO_READ;

  // We divide the transmitting stream of bytes in chunks with a maximun length of 65536 bytes each.
  while(len > 0) 
  {
    cur_chunk_len = min(len, 65536);
    len = len - cur_chunk_len;
    cur_command_size = cur_chunk_len + 3;

    /// Low and High bytes of the length field
    mybuf[1] = (unsigned char) (cur_chunk_len - 1);
    mybuf[2] = (unsigned char) ((cur_chunk_len - 1) >> 8);

    debug("\tOPCODE:  0x%x\n", mybuf[0]);
    debug("\tLENGTL:  0x%02x\n", mybuf[1]);
    debug("\tLENGTH:  0x%02x\n", mybuf[2]);

    /// The rest of the command is filled with the bytes that will be transferred
    memcpy(&(mybuf[3]), buf, cur_chunk_len);
    buf = buf + cur_chunk_len;
    for(i = 0; i< cur_chunk_len; i++)
      if(debug>1) debug("\tBYTE%3d: 0x%02x\n", i, mybuf[3+i]);

    /// Finally we can ransmit the command
    xferred = usbconn_ftdi_write(conn, mybuf, cur_command_size, (postread ? cur_chunk_len : 0));
    if(xferred != cur_command_size)
      return -1;

    // If OK, the update the number of incoming bytes that are being buffered for a posterior read
    if(postread)
      recv = recv + cur_chunk_len;
  }
  debug("\tPOSTREAD: %u bytes\n", recv);

  // Returns the number of buffered incoming bytes
  return recv;
}

int cable_ft2232_write_bits(usbconn_t *conn, unsigned char *buf, int len, int postread, int with_tms)
{
  int max_command_size;
  int max_chunk_len;
  int cur_chunk_len;
  int recv;
  int xferred;
  int i;
  unsigned char *mybuf;

  if(len == 0)
    return 0;

  max_command_size = 3;
  mybuf = (unsigned char *) malloc(max_command_size);

  if(!with_tms) 
  {
    /// Command OPCODE: write bits (can write up to 8 bits in a single command)
    max_chunk_len = 8;
    mybuf[0] = MPSSE_DO_WRITE | MPSSE_LSB | MPSSE_WRITE_NEG | MPSSE_BITMODE;
  } 
  else 
  {
    /// Command OPCODE: 0x4B write bit with tms (can write up to 1 bits in a single command)
    max_chunk_len = 1;
    mybuf[0] = MPSSE_WRITE_TMS|MPSSE_LSB|MPSSE_BITMODE|MPSSE_WRITE_NEG;
  }

  if(postread) // (OPCODE += 0x20) if postread is enabled it will buffer incoming bits
    mybuf[0] = mybuf[0] | MPSSE_DO_READ;

  // We divide the transmitting stream of bytes in chunks with a maximun length of max_chunk_len bits each.
  i = 0;
  recv = 0;
  while(len > 0) 
  {
    cur_chunk_len = min(len, max_chunk_len);
    len = len - cur_chunk_len;

    /// Bits length field
    mybuf[1] = (unsigned char) (cur_chunk_len - 1);

    debug("\tOPCODE:  0x%x\n", mybuf[0]);
    debug("\tLENGTH:  0x%02x\n", mybuf[1]);

    if(!with_tms) 
    {
      /// The last byte of the command is filled with the bits that will be transferred
      debug("\tDATA[%d]  0x%02x\n", (i/8), buf[i/8]);
      mybuf[2] = buf[i/8];
      i = i+8;
    }
    else 
    {
      //TODO: seleccionar el bit a transmitir
      mybuf[2] = 0x01 | ((buf[(i/8)] >> (i%8)) << 7);
      i++;
    }

    debug("\tBYTE%3d: 0x%02x\n", i, mybuf[2]);

    /// Finally we can transmmit the command
    xferred = usbconn_ftdi_write(conn, mybuf, max_command_size, (postread ? 1 : 0));
    if(xferred != max_command_size)
      return -1;

    // If OK, the update the number of incoming bytes that are being buffered for a posterior read
    if(postread) 
      recv = recv + 1;
  }
  debug("\tPOSTREAD: %u bytes\n", recv);

  return recv;
}

int cable_ft2232_read_packed_bits(usbconn_t *conn, uint8_t *buf, int packet_len, int bits_per_packet, int offset)
{
  unsigned char *mybuf;
  unsigned char dst_mask;
  unsigned char src_mask;
  int row_offset;
  int dst_row;
  int dst_col;
  int src_row;
  int src_col;
  int i;
  int r;

  if(packet_len == 0 || bits_per_packet == 0)
    return 0;

  mybuf = (unsigned char *) malloc(packet_len);
  if((r=usbconn_ftdi_read(conn, mybuf, packet_len)) < 0) 
  {
    debug("Read failed\n");
    return -1;
  }

  if(bits_per_packet < 8) 
  {
    // rotate bits to the left side
    for(i = 0; i < packet_len; i++)
    {
//      debug("[MYDBG] unaligned bits[%d]=%02x\n", i, mybuf[i]);      
      mybuf[i] = (mybuf[i] >> (8-bits_per_packet));
//      debug("[MYDBG]   aligned bits[%d]=%02x\n", i, mybuf[i]);
    }
    for(i = offset; i < (packet_len*bits_per_packet+offset); i++) 
    {
      dst_row = i / 8;
      dst_col = i % 8;
      src_row = (i-offset) / bits_per_packet;
      src_col = (i-offset) % bits_per_packet;
      dst_mask = ~(1 << dst_col);
      src_mask = (1 << src_col);
//      debug("[MYDBG] i = %4d dst[%3d][%3d] dst_mask=%02x dst_val=%02x dst_masked=%02x\n", i, dst_row, dst_col, dst_mask, buf[dst_row], (buf[dst_row] & dst_mask));
//      debug("[MYDBG] i = %4d src[%3d][%3d] src_mask=%02x src_val=%02x src_masked=%02x\n", i, src_row, src_col, src_mask, mybuf[src_row], (mybuf[src_row] & src_mask));
      if(dst_col >= src_col)
        buf[dst_row] = (buf[dst_row] & dst_mask) | ((mybuf[src_row] & src_mask) << (dst_col - src_col));
      else
        buf[dst_row] = (buf[dst_row] & dst_mask) | ((mybuf[src_row] & src_mask) >> (dst_col - src_col));
    }   
  } 
  else if(bits_per_packet == 8)
  {
    row_offset = offset / 8;
//    debug("[MYDBG] Row offset=%d\n", row_offset);
    memcpy(&(buf[row_offset]), mybuf, packet_len);
  }
  else 
    return -1;

//  debug("read_bits()-> %x\n", *buf);
  return ((r < 1) ? -1 : 0);
}

int cable_ft2232_write_stream(usbconn_t *conn, unsigned char *buf, int len, int postread, int with_tms) 
{
  int len_bytes;
  int len_bits;
  int len_tms_bits;
  unsigned char mybuf;

  len_tms_bits = ((with_tms) ? 1 : 0);
  len_bytes = ((len -len_tms_bits) / 8);
  len_bits  = ((len -len_tms_bits) % 8);

  debug("[MYDBG] cable_ft2232_write_stream(len=%d postread=%d tms=%d) = %d bytes %dbits %dtms_bits\n", len, postread, with_tms, len_bytes, len_bits, len_tms_bits);

  if(len_bytes > 0)
    cable_ft2232_write_bytes(conn, buf, len_bytes, postread);

  if(len_bits > 0)
    cable_ft2232_write_bits(conn, &(buf[len_bytes]), len_bits, postread, 0);

  if(len_tms_bits > 0) 
  {
    mybuf = (buf[len_bytes] >> len_bits);
    cable_ft2232_write_bits(conn, &mybuf, 1, postread, 1);
  }

  return 0;
}

int cable_ft2232_read_stream(usbconn_t *conn, unsigned char *buf, int len, int with_tms)
{
  int len_bytes;
  int len_bits;
  int len_tms_bits;

  len_tms_bits = ((with_tms) ? 1 : 0);
  len_bytes = ((len -len_tms_bits) / 8);
  len_bits  = ((len -len_tms_bits) % 8);

  debug("[MYDBG] cable_ft2232_read_stream(len=%d tms=%d) = %d bytes %dbits %dtms_bits\n", len, with_tms, len_bytes, len_bits, len_tms_bits);

  if(len_bytes > 0)
    cable_ft2232_read_packed_bits(conn, buf, len_bytes, 8, 0);

  if(len_bits > 0)
    cable_ft2232_read_packed_bits(conn, buf, 1, len_bits, (len_bytes * 8));

  if(len_tms_bits > 0)
    cable_ft2232_read_packed_bits(conn, buf, 1, 1, (len_bits + (len_bytes * 8)));

  return 0;
}



/// ----------------------------------------------------------------------------------------------
/// Advanced Jtag debugger driver interface.
/// ----------------------------------------------------------------------------------------------

jtag_cable_t *cable_ftdi_get_driver(void)
{
  return &ft2232_cable_driver; 
}

int cable_ftdi_init() 
{
  int err = APP_ERR_NONE;
  int res = 0;
  unsigned char  *buf = malloc(10);

  ft2232_device = usbconn_ftdi_connect();

  if((res = usbconn_ftdi_mpsse_open(ft2232_device)) != 0)
    err |= APP_ERR_USB;
  printf("Open MPSSE mode returned: %s\n", ((res != 0) ? "FAIL" : "OK"));

  ftdi_param_t *params = ft2232_device->params;
  //struct ftdi_context * ftdic = params->ftdic;

  buf[0]= SET_BITS_LOW;  // Set value & direction of ADBUS lines
  buf[1]= 0x00;          // values
  buf[2]= 0x1b;          // direction (1 == output)
  buf[3]= TCK_DIVISOR;
  buf[4]= 0x01;
  buf[5]= 0x00;
  buf[6]= SET_BITS_HIGH;
  buf[7]= ~0x04;
  buf[8]= 0x04;
  buf[9]= SEND_IMMEDIATE;
  if(usbconn_ftdi_write(ft2232_device , buf, 10, 0) != 10) 
  {
    err |= APP_ERR_USB;
    printf("Initial write failed\n");
  }

  usbconn_ftdi_flush(params);

  return err;
}

int cable_ftdi_close() 
{
  usbconn_ftdi_close(ft2232_device);
  usbconn_ftdi_free(ft2232_device);
  
  return APP_ERR_NONE;
}

int cable_ftdi_flush() 
{
  ftdi_param_t *params = ft2232_device->params;
  usbconn_ftdi_flush(params);

  return APP_ERR_NONE;
}

int cable_ftdi_write_bit(uint8_t packet) 
{
  int err = APP_ERR_NONE;
  unsigned char buf;
  int tms;

  buf = ((packet & TDO) ? 0x01 : 0x00);
  tms = ((packet & TMS) ? 1 : 0);

  if(cable_ft2232_write_stream(ft2232_device, &buf, 1, 0, tms) < 0)
    err |= APP_ERR_COMM;

  cable_ftdi_flush();

  return err; 
}

int cable_ftdi_read_write_bit(uint8_t packet_out, uint8_t *bit_in) 
{

  int err = APP_ERR_NONE;
  unsigned char buf;
  int tms;

  buf = ((packet_out & TDO) ? 0x01 : 0x00);
  tms = ((packet_out & TMS) ? 1 : 0);

  if(cable_ft2232_write_stream(ft2232_device, &buf, 1, 1, tms) < 0)
    err = APP_ERR_COMM;

  if(cable_ft2232_read_stream(ft2232_device, ((unsigned char *)bit_in), 1, tms) < 0)
    err = APP_ERR_COMM;
  
  return err;
}

int cable_ftdi_write_stream(uint32_t *stream, int len_bits, int set_last_bit) 
{
  int err = APP_ERR_NONE;

  if(cable_ft2232_write_stream(ft2232_device, ((unsigned char *)stream), len_bits, 0, set_last_bit) < 0)
    err |= APP_ERR_COMM;

  cable_ftdi_flush();

  return err;
}

int cable_ftdi_read_stream(uint32_t *outstream, uint32_t *instream, int len_bits, int set_last_bit) 
{
  int err = APP_ERR_NONE;
  if(cable_ft2232_write_stream(ft2232_device, ((unsigned char *)outstream), len_bits, 1, set_last_bit) < 0)
    err |= APP_ERR_COMM;
  if(cable_ft2232_read_stream(ft2232_device, ((unsigned char *)instream), len_bits, set_last_bit) < 0)
    err |= APP_ERR_COMM;

  return err;
}

int cable_ftdi_opt(int c, char *str) 
{
  uint32_t newvid;
  uint32_t newpid;

  switch(c) {
    case 'p':
      if(!sscanf(str, "%x", &newpid)) 
      {
        fprintf(stderr, "p parameter must have a hex number as parameter\n");
        return APP_ERR_BAD_PARAM;
      }
      else 
        usbconn_ft2232_mpsse_CableID2.pid = newpid;
      break;

    case 'v':
      if(!sscanf(str, "%x", &newvid)) 
      {
        fprintf(stderr, "v parameter must have a hex number as parameter\n");
        return APP_ERR_BAD_PARAM;
      }
      else 
        usbconn_ft2232_mpsse_CableID2.vid = newvid;
      break;

    default:
      fprintf(stderr, "Unknown parameter '%c'\n", c);
      return APP_ERR_BAD_PARAM;
  }
  return APP_ERR_NONE;
}

/// ----------------------------------------------------------------------------------------------

